// STL
#include <algorithm>                              // std::max(), ::min_element(), ::max_element()
#include <cmath>                                  // std::sqrt()
#include <functional>                             // std::function<>
#include <utility>                                // std::pair<>, ::make_pair()
#include <vector>                                 // std::vector<>

// jScience
#include "jScience/stl/vector.hpp"                // std::vector<>*=

// stats++
#include "statsxx/statistics.hpp"                 // statistics::mean()

// this
#include "statsxx/optimization/EA/Population.hpp" // Population


//
// DESC: Calculate fitnesses (of the entire population).
//
// =====
//
// NOTE: This subroutine implements the self-adaptive penalty function algorithm described in the following:
//
//     B. Tessema and G. G. Yen, "A Self Adaptive Penalty Function Based Algorithm for Constrained Optimization," IEEE International Conference on Evolutionary Computation (2006)
//
//     B. Tessema, "A self-adaptive genetic algorithm for constrained optimization," thesis (2004)
//
// NOTE: Also used is the constraint violation in:
//
//     R. Mallipeddi and P. N. Suganthan, "Ensemble of Constraint Handling Techniques," IEEE Transactions on Evolutionary Computation 14, 561--579 (2010)
//
// =====
//
// TODO: NOTE: equality_constraints is NOT used AT ALL in this subroutine.
//
// =====
//
// TODO: Should NOT be operating on arguments!
//
inline std::pair<
                 std::vector<double>, // f
                 std::vector<double>  // v
                 > calc_fitness(
                                Population                                                            &population,
                                // =====
                                const std::function<double(const std::vector<double> &)>              &fitness,
                                // -----
                                const std::vector<std::function<double(const std::vector<double> &)>> &inequality_constraints,
                                const std::vector<std::function<double(const std::vector<double> &)>> &equality_constraints
                                )
{
    // NOTE: It is never explicitly specified in B. Tessema (thesis) whether the maximum violation constraint is calculated for each generation.
    //
    // NOTE: According to Eq. (3) in R. Mallipeddi and P. N. Suganthan (2010), the weights that enter the violation constraint are defined by the maximum violation constraint encountered during the evolution.
    //
    // -----
    //
    // NOTE: Initializing this std::vector<> to 0 provides a simple check for whether this constraint has been violated yet.
    //
    static std::vector<double> c_max(inequality_constraints.size(), 0.);

    //=========================================================

    std::vector<double> f;
    std::vector<double> v;

    //=========================================================
    // STEP 1. FITNESS
    //=========================================================

    // CALCULATE (STANDARD) FITNESS VALUES
    //
    // NOTE: Even with no feasible individuals, (standard) fitness values are needed for return.
    //
    for( auto &ind : population.individuals )
    {
        double _f = fitness(
                            ind.genome
                            );

        // "CONVERT" fitness() TO A FUNCTION TO BE *MINIMIZED*
        //
        // NOTE: fitness(), by definition by EA, is a function to be *maximized*.
        //
        // NOTE: The Tessema and Yen (2006) approach, however, is formulated for *minimization*.
        //
        // -----
        //
        // NOTE: A function to be maximized can be turned into one to minimize (and vice versa) by multiplying by (-1).
        //
        _f *= (-1);

        // NOTE: The (final) fitness function will be (below) converted back to one to to maximize.

        f.push_back(_f);
    }

    // NOTE: If r_f = 0 (calculated below), then normalized fitness values are not needed.

    //=========================================================
    // STEP 2. CONSTRAINT VIOLATIONS
    //=========================================================

    std::vector<std::vector<double>> c;

    // INEQUALITY CONSTRAINTS
    for( auto j = 0; j < inequality_constraints.size(); ++j )
    {
        std::vector<double> cj;

        for( auto &ind : population.individuals )
        {
            double _cj = inequality_constraints[j](
                                                   ind.genome
                                                   );

            _cj = std::max(0., _cj);

            cj.push_back(_cj);
        }

        c.push_back(cj);

        double cj_max = *std::max_element(cj.begin(), cj.end());

        c_max[j] = std::max(c_max[j], cj_max);
    }

    // CALCULATE v (CONSTRAINT VIOLATION)
    v = std::vector<double>(population.individuals.size(), 0.);

    for( auto i = 0; i < population.individuals.size(); ++i )
    {
        for( auto j = 0; j < inequality_constraints.size(); ++j )
        {
            // NOTE: The check whether ( c[j][i] != 0. ) ensures that (c_max[j] ! = 0).
            //
            if( c[j][i] != 0. )
            {
                v[i] += c[j][i]/c_max[j];
            }
        }
    }

    // NORMALIZE
    //
    // NOTE: The B. Tessema (thesis) uses normalization by the total number of constraints.
    //
    // NOTE: R. Mallipeddi and P. N. Suganthan (2010), on the other hand, uses a weighted version that balances the contribution of every constraint and that normalizes to 1.
    //
    double v_denom = 0.;

    for( auto j = 0; j < inequality_constraints.size(); ++j )
    {
        // NOTE: If ( c_max[j] == 0. ), then there is no contribution to v[] (above).
        //
        // NOTE: ... (I.e., the normalization is consistent.)
        //
        if( c_max[j] != 0. )
        {
            v_denom += (1./c_max[j]);
        }
    }

    if( v_denom != 0. )
    {
        v /= v_denom;
    }

    //=========================================================
    // STEP 3. FEASIBLE INDIVIDUALS
    //=========================================================

    int n_f = 0;

    for( auto i = 0; i < population.individuals.size(); ++i )
    {
        if( v[i] == 0. )
        {
            ++n_f;
        }
    }

    double r_f = static_cast<double>(n_f)/population.individuals.size();

    //=========================================================
    // STEP 4. DISTANCES AND PENALTIES
    //=========================================================

    std::vector<double> d(population.individuals.size());
    std::vector<double> p(population.individuals.size(), 0.);

    if( n_f == 0 )
    {
        // NOTE: If n_f (and hence r_f) = 0, then d[] = v[], and p[] = 0.

        d = v;
    }
    else
    {
        //---------------------------------------------------------
        // NORMALIZED FITNESS VALUES
        //---------------------------------------------------------
        std::vector<double> f_norm;

        // NOTE: According to the pseudocode of the proposed algorithm (and corresponding discussion) in B. Tessema (thesis) (Figure 4.6), f_min and f_max are calculated using fitness values for the current generation.
        //
        double f_min = *std::min_element(f.begin(), f.end());
        double f_max = *std::max_element(f.begin(), f.end());

        if( f_min != f_max )
        {
            for( auto &fi : f )
            {
                double _f_norm = (fi - f_min)/(f_max - f_min);

                f_norm.push_back(_f_norm);
            }
        }
        else
        {
            f_norm = std::vector<double>(population.individuals.size(), 0.);
        }

        //---------------------------------------------------------
        // DISTANCE
        //---------------------------------------------------------

        for( auto i = 0; i < population.individuals.size(); ++i )
        {
            d[i] = std::sqrt(f_norm[i]*f_norm[i] + v[i]*v[i]);
        }

        //---------------------------------------------------------
        // PENALTY (X)
        //---------------------------------------------------------

        std::vector<double> X = v;

        //---------------------------------------------------------
        // PENALTY (Y)
        //---------------------------------------------------------

        std::vector<double> Y(population.individuals.size());

        for( auto i = 0; i < population.individuals.size(); ++i )
        {
            if( v[i] == 0. )
            {
                Y[i] = 0.;
            }
            else
            {
                Y[i] = f_norm[i];
            }
        }

        //---------------------------------------------------------
        // p
        //---------------------------------------------------------

        // NOTE: B. Tessema (thesis) Eq. (4.6).
        //
        p = (1. - r_f)*X + r_f*Y;
    }

    //=========================================================
    // FINAL FITNESS
    //=========================================================

    // NOTE: B. Tessema (thesis) Eq. (4.7).
    //
    std::vector<double> f_final = d + p;

    // CONVERT (TOTAL) FITNESS FUNCTION TO ONE TO BE MAXIMIZED
    //
    f_final *= (-1.);

    // ... SAME FOR (RAW) FITNESS
    f *= (-1.);

    //=========================================================
    // ASSIGN FITNESS (FOR AND RETURN)
    //=========================================================

    for( auto i = 0; i < population.individuals.size(); ++i )
    {
        population.individuals[i].fitness = f_final[i];
    }

    population.avg_fitness = statistics::mean(f_final);
    population.max_fitness = *std::max_element(f_final.begin(), f_final.end());

    return std::make_pair(
                          f,
                          v
                          );
}
